from collections import defaultdict
from math import sqrt
class Brain:
    def LoadNodes(self):
        #Open file
        file = open("locations.csv")
        #Read first line with headers
        dummy = file.readline()
        #Loop through lines, remove end-line character, split and create new node
        for line in file:
            line = line[:len(line)-1]
            (id, name, x, y) = line.split(';')
            self.nodes.append(Node(id, name, int(x), int(y)))

    def LoadLinks(self):
        #open file
        file = open("links.csv")
        #Remove header line
        dummy = file.readline()
        #Loop through lines, remove end-line character, split and create new link
        for line in file:
            line = line[:len(line)-1]
            (id, firstId, secondId) = line.split(";")
            self.links.append(Link(id, firstId, secondId))

    def BuildRoadMap(self):
        #For all nodes, loop through linkes, and add the node the node to roadMap, as a multidict. Ex, roadMap[nodeId] = [(road1, road2, road3)]
        for node in self.nodes:
            for link in self.links:
                if ((link.start == node.id) or (link.end == node.id)):
                    self.roadMap[node.id].add(link.id)

    def toString(self):
        #Print road-map to command-line. Debug only. LOTS of data. 
        for k, v in self.roadMap.items():
            print("roadmap[%s] = '%s'" % (k, v))
            print("\n\n")

    def getNode(self, name):
        #Turn symbolic name for node into node object
        for node in self.nodes:
            if node.name == name:
                return node
        return "Wrong name!"

    def getAllLinksFrom(self, node):
        #Get all links from a node as a set
        links = set()
        #Get linknumbers from roadmap multi dict.
        linkNums = self.roadMap[node.id]
        #For all links, if it is in the linknumbers, add it to link set
        for link in self.links:
            if link.id in linkNums:
                links.add(link)
        return links

    def getLocations(self, link):
        start = ""
        end = ""
        #For all nodes, if node is either link-start or link end, set it
        for node in self.nodes:
            if node.id == link.start:
                start = node
            if node.id == link.end:
                end = node
        #If found both start and end, return as tuple. Else return error
        if start and end:
            return (start, end)
        else:
            return "Bad link"


    def getBest(self, frontier):
        #Return the best from frontier
        current = ""
        cost = 10000;
        #Find item with lowest estimate
        for item in frontier:
            if item[1] < cost:
               current = item
               cost = item[1]
        frontier.remove(current)
        return current;
    
    #Rebuild path, starting from endnode
    def reconstructPath(self, path, endNode):
        #Reconstruct path
        reconstructed = list()
        #Add endNode
        reconstructed.append(endNode)
        #Find previousStop before end node
        previousStop = path[endNode]
        #Get distance, actual
        distance = previousStop[1]
        #As long as we have a previousNode, loop
        while previousStop[0] in path:
            previousNode = previousStop[0]
            reconstructed.append(previousNode)
            previousStop = path[previousStop[0]]
        reconstructed.reverse()
        #Return path as node-list, and distance as double
        return (reconstructed, distance)
    
    #A-star search
    def astar(self, startName, endName):
        #Setup 
        startNode = self.getNode(startName)
        endNode = self.getNode(endName)
        if (endNode == "Wrong name!" or startNode == "Wrong name!"):
            return (0, "Wrong name!")
        frontier = list()

        #Prepare frontier
        heur = self.H(startNode, endNode)
        frontier.append((startNode, heur))

        #Prepare path
        path = dict()
        path[startNode] = (Node(0, "Start", 0,0),0)


        while frontier:
            best = self.getBest(frontier)
            bestNode = best[0]
            previousNode = path[bestNode][0]
            #If the best is also the one we seek
            if bestNode==endNode:
                return self.reconstructPath(path, endNode)

            #Else move frontier
            links = self.getAllLinksFrom(bestNode)
            for link in links:
                (firstNode, secondNode) = self.getLocations(link)
                actualNode = ""
                #Make sure is not loop-back
                if(firstNode != secondNode):
                    #Not sure of direction of road: get the end we seek
                    if (firstNode != bestNode):
                       actualNode = firstNode
                    else:
                       actualNode = secondNode

                    #Calculate traversed distance: recursive lookup (sort of)
                    traversed = path[bestNode][1] + self.G(bestNode, actualNode)
                    #Do heuristic
                    estimate = self.H(actualNode, endNode)
                    sum = traversed + estimate

                    #If we've already visited node, check if new sum is better. If so, add again to frontier and update path.
                    if (actualNode not in path or sum < path[actualNode][1]):
                        frontier.append( (actualNode, sum) )
                        path[actualNode] = (bestNode, traversed)

        #If frontier is empty without finding a path
        return 0
    
    #A-star search
    def reachable(self, startName, endName):
        #Setup 
        startNode = self.getNode(startName)
        endNode = self.getNode(endName)
        frontier = list()

        #Prepare frontier
        frontier.append(startNode)

        #Prepare path
        path = dict()
        path[startNode] = (Node(0, "Start", 0,0),0)


        while frontier:
            currentNode = frontier.pop()
            #If the best is also the one we seek
            if currentNode == endNode:
                return self.reconstructPath(path, endNode)

            #Else move frontier
            links = self.getAllLinksFrom(currentNode)
            for link in links:
                (firstNode, secondNode) = self.getLocations(link)
                actualNode = ""
                #Make sure is not loop-back
                if(firstNode != secondNode):
                    #Not sure of direction of road: get the end we seek
                    if (firstNode != currentNode):
                       actualNode = firstNode
                    else:
                       actualNode = secondNode

                    
                    traversed = path[currentNode][1] + self.G(currentNode, actualNode)
                    #If we've already visited node, check if new sum is better. If so, add again to frontier and update path.
                    if (actualNode not in path):
                        frontier.append(actualNode)
                        path[actualNode] = ((currentNode, traversed))

        #If frontier is empty without finding a path
        return 0

    #Heuristic: pythagoras theorem from node1 to node2
    def H(self, node1, node2):
        xdist = node1.x-node2.x
        ydist = node1.y-node2.y
        pyth = sqrt(xdist*xdist+ydist*ydist)
        return pyth
    
    #Actual distance from node1 to node2: in practice, pythagoras.
    def G(self, node1, node2):
        xdist = node1.x-node2.x
        ydist = node1.y-node2.y
        pyth = sqrt(xdist*xdist+ydist*ydist)
        return pyth
    
    #Setup member variables
    def __init__(self):
        self.nodes = list()
        self.links = list()
        #Multidict
        self.roadMap = defaultdict(set)


class Node:
    #Setup member variables
    def __init__(self, id, name, x, y):
        self.id = id
        self.name = name
        self.x = x
        self.y = y

class Link:
    #Setup member variables
    def __init__ (self, id, start, end):
        self.id = id
        self.start = start
        self.end = end

B = Brain()
B.LoadNodes()
B.LoadLinks()
B.BuildRoadMap()
print("Type 'quit' to quit")
locA = input('Type first location:')
locB = input('Type second location:')
while (locA != 'quit' and locB != 'quit'):
    
    (path,distance) = B.reachable(locA, locB)
    if path == 0:
        print ("Failure!")
        print(distance)
    else:
        print("The breadth-first route from %s to %s is %d distance units" % (locA, locB, distance))
        for p in path:
            print (p.name)

    (path, distance) = B.astar(locA, locB)
    if path == 0:
        print ("Failure!")
        print(distance)
    else:
        print("The optimal route from %s to %s is %d distance units" % (locA, locB, distance))
        for p in path:
            print (p.name)
    
    print("Type 'quit' to quit")
    locA = input('Type first location:')
    locB = input('Type second location:')